﻿namespace MyTRCP.Common.Codecs.Sbc
{
    public class SbcDecoder
    {
        private const uint SBCDEC_FIXED_EXTRA_BITS = 2;
        private const int SCALE4_STAGED1_BITS = 15;
        private const int SCALE4_STAGED2_BITS = 16;
        private const int SCALE8_STAGED1_BITS = 15;
        private const int SCALE8_STAGED2_BITS = 16;
        private static readonly int[,] synmatrix4 = new int[8, 4] { { 5792, -5793, -5793, 5792 }, { 3134, -7569, 7568, -3135 }, { 0, 0, 0, 0 }, { -3135, 7568, -7569, 3134 }, { -5793, 5792, 5792, -5793 }, { -7569, -3135, 3134, 7568 }, { -8192, -8192, -8192, -8192 }, { -7569, -3135, 3134, 7568 } };
        private static readonly int[,] synmatrix8 = new int[16, 8] { { 5792, -5793, -5793, 5792, 5792, -5793, -5793, 5792 }, { 4551, -8035, 1598, 6811, -6812, -1599, 8034, -4552 }, { 3134, -7569, 7568, -3135, -3135, 7568, -7569, 3134 }, { 1598, -4552, 6811, -8035, 8034, -6812, 4551, -1599 }, { 0, 0, 0, 0, 0, 0, 0, 0 }, { -1599, 4551, -6812, 8034, -8035, 6811, -4552, 1598 }, { -3135, 7568, -7569, 3134, 3134, -7569, 7568, -3135 }, { -4552, 8034, -1599, -6812, 6811, 1598, -8035, 4551 }, { -5793, 5792, 5792, -5793, -5793, 5792, 5792, -5793 }, { -6812, 1598, 8034, 4551, -4552, -8035, -1599, 6811 }, { -7569, -3135, 3134, 7568, 7568, 3134, -3135, -7569 }, { -8035, -6812, -4552, -1599, 1598, 4551, 6811, 8034 }, { -8192, -8192, -8192, -8192, -8192, -8192, -8192, -8192 }, { -8035, -6812, -4552, -1599, 1598, 4551, 6811, 8034 }, { -7569, -3135, 3134, 7568, 7568, 3134, -3135, -7569 }, { -6812, 1598, 8034, 4551, -4552, -8035, -1599, 6811 } };
        private static readonly int[] sbc_proto_4_40m0 = new int[20] { 0, -1431, -17773, 17772, 1430, -71, -2679, -25558, 10177, 401, -196, -3785, -32328, 3777, -245, -359, -4220, -36940, -804, -511 };
        private static readonly int[] sbc_proto_4_40m1 = new int[20] { -503, -3392, -38577, -3392, -503, -511, -804, -36940, -4220, -359, -245, 3777, -32328, -3785, -196, 401, 10177, -25558, -2679, -71 };
        private static readonly int[] sbc_proto_8_80m0 = new int[40] { 0, -1484, -17826, 17825, 1483, -42, -2105, -21754, 13942, 916, -90, -2742, -25579, 10243, 432, -146, -3342, -29150, 6844, 46, -216, -3842, -32314, 3837, -237, -299, -4170, -34935, 1288, -424, -388, -4253, -36898, -767, -523, -468, -4016, -38114, -2322, -552 };
        private static readonly int[] sbc_proto_8_80m1 = new int[40] { -528, -3392, -38524, -3392, -528, -552, -2322, -38114, -4016, -468, -523, -767, -36898, -4253, -388, -424, 1288, -34935, -4170, -299, -237, 3837, -32314, -3842, -216, 46, 6844, -29150, -3342, -146, 432, 10243, -25579, -2742, -90, 916, 13942, -21754, -2105, -42 };
        private readonly SbcConfig _config;
        private readonly SbcDecoderState _state;
        private bool _init;
        private readonly SbcFrame _frame;
        public SbcDecoder(bool isBigEndian)
        {
            _config = isBigEndian ? SbcConfig.CreateMyVoiceSbcConfigBE() : SbcConfig.CreateMyVoiceSbcConfigLE();
            _state = new SbcDecoderState();
            _frame = new SbcFrame();
        }
        public SbcDecoder(SbcConfig config)
        {
            _config = config;
            _state = new SbcDecoderState();
            _frame = new SbcFrame();
        }
        private void SbcDecoderInit()
        {
            _state.V = new int[2, 170];
            _state.Subbands = _frame.Subbands;
            for (int ch = 0; ch < 2; ch++)
                for (int i = 0; i < _frame.Subbands * 2; i++)
                    _state.Offset[ch, i] = 10 * i + 10;
        }
        private void SbcSynthesizeFour(int ch, int blk)
        {
            int i, j, k, idx;

            for (i = 0; i < 8; i++)
            {
                /* Shifting */
                _state.Offset[ch, i]--;
                if (_state.Offset[ch, i] < 0)
                {
                    _state.Offset[ch, i] = 79;
                    for (j = 0; j < 9; j++)
                        _state.V[ch, j + 80] = _state.V[ch, j];//memcpy(v + 80, v, 9 * sizeof(*v));
                }

                /* Distribute the new matrix value to the shifted position */
                _state.V[ch, _state.Offset[ch, i]] =
                    synmatrix4[i, 0] * _frame.SbSample[blk, ch, 0] +
                    synmatrix4[i, 1] * _frame.SbSample[blk, ch, 1] +
                    synmatrix4[i, 2] * _frame.SbSample[blk, ch, 2] +
                    synmatrix4[i, 3] * _frame.SbSample[blk, ch, 3] >> SCALE4_STAGED1_BITS;
            }

            /* Compute the samples */
            for (idx = 0, i = 0; i < 4; i++, idx += 5)
            {
                k = i + 4 & 0xf;

                /* Store in output, Q0 */
                _frame.PcmSample[ch, blk * 4 + i] = SbcCommon.SbcClip16(
                    _state.V[ch, _state.Offset[ch, i] + 0] * sbc_proto_4_40m0[idx + 0] +
                    _state.V[ch, _state.Offset[ch, k] + 1] * sbc_proto_4_40m1[idx + 0] +
                    _state.V[ch, _state.Offset[ch, i] + 2] * sbc_proto_4_40m0[idx + 1] +
                    _state.V[ch, _state.Offset[ch, k] + 3] * sbc_proto_4_40m1[idx + 1] +
                    _state.V[ch, _state.Offset[ch, i] + 4] * sbc_proto_4_40m0[idx + 2] +
                    _state.V[ch, _state.Offset[ch, k] + 5] * sbc_proto_4_40m1[idx + 2] +
                    _state.V[ch, _state.Offset[ch, i] + 6] * sbc_proto_4_40m0[idx + 3] +
                    _state.V[ch, _state.Offset[ch, k] + 7] * sbc_proto_4_40m1[idx + 3] +
                    _state.V[ch, _state.Offset[ch, i] + 8] * sbc_proto_4_40m0[idx + 4] +
                    _state.V[ch, _state.Offset[ch, k] + 9] * sbc_proto_4_40m1[idx + 4] >> SCALE4_STAGED1_BITS);
            }
        }
        private void SbcSynthesizeEight(int ch, int blk)
        {
            int i, j, k, idx;

            for (i = 0; i < 16; i++)
            {
                /* Shifting */
                _state.Offset[ch, i]--;
                if (_state.Offset[ch, i] < 0)
                {
                    _state.Offset[ch, i] = 159;
                    for (j = 0; j < 9; j++)
                        _state.V[ch, j + 160] = _state.V[ch, j];
                }

                /* Distribute the new matrix value to the shifted position */
                _state.V[ch, _state.Offset[ch, i]] =
                    synmatrix8[i, 0] * _frame.SbSample[blk, ch, 0] +
                    synmatrix8[i, 1] * _frame.SbSample[blk, ch, 1] +
                    synmatrix8[i, 2] * _frame.SbSample[blk, ch, 2] +
                    synmatrix8[i, 3] * _frame.SbSample[blk, ch, 3] +
                    synmatrix8[i, 4] * _frame.SbSample[blk, ch, 4] +
                    synmatrix8[i, 5] * _frame.SbSample[blk, ch, 5] +
                    synmatrix8[i, 6] * _frame.SbSample[blk, ch, 6] +
                    synmatrix8[i, 7] * _frame.SbSample[blk, ch, 7] >> SCALE8_STAGED1_BITS;
            }

            /* Compute the samples */
            for (idx = 0, i = 0; i < 8; i++, idx += 5)
            {
                k = i + 8 & 0xf;

                /* Store in output, Q0 */
                _frame.PcmSample[ch, blk * 8 + i] = SbcCommon.SbcClip16(
                    _state.V[ch, _state.Offset[ch, i] + 0] * sbc_proto_8_80m0[idx + 0] +
                    _state.V[ch, _state.Offset[ch, k] + 1] * sbc_proto_8_80m1[idx + 0] +
                    _state.V[ch, _state.Offset[ch, i] + 2] * sbc_proto_8_80m0[idx + 1] +
                    _state.V[ch, _state.Offset[ch, k] + 3] * sbc_proto_8_80m1[idx + 1] +
                    _state.V[ch, _state.Offset[ch, i] + 4] * sbc_proto_8_80m0[idx + 2] +
                    _state.V[ch, _state.Offset[ch, k] + 5] * sbc_proto_8_80m1[idx + 2] +
                    _state.V[ch, _state.Offset[ch, i] + 6] * sbc_proto_8_80m0[idx + 3] +
                    _state.V[ch, _state.Offset[ch, k] + 7] * sbc_proto_8_80m1[idx + 3] +
                    _state.V[ch, _state.Offset[ch, i] + 8] * sbc_proto_8_80m0[idx + 4] +
                    _state.V[ch, _state.Offset[ch, k] + 9] * sbc_proto_8_80m1[idx + 4] >> SCALE8_STAGED1_BITS);
            }
        }
        private int SbcSynthesizeAudio()
        {
            int ch, blk;
            switch (_frame.Subbands)
            {
                case 4:
                    for (ch = 0; ch < _frame.Channels; ch++)
                    {
                        for (blk = 0; blk < _frame.Blocks; blk++)
                            SbcSynthesizeFour(ch, blk);
                    }
                    return _frame.Blocks * 4;

                case 8:
                    for (ch = 0; ch < _frame.Channels; ch++)
                    {
                        for (blk = 0; blk < _frame.Blocks; blk++)
                            SbcSynthesizeEight(ch, blk);
                    }
                    return _frame.Blocks * 8;

                default:
                    return -5;
            }
        }
        public int SbcDecode(byte[] input, int inputPos, int inputLen, byte[] output, int outputPos, int outputLen, ref int written)
        {
            if (_config == null || input == null)
                return -5;

            int framelen = SbcUnpackFrame(input, inputPos, inputLen);

            if (framelen <= 0)
                return framelen;

            if (!_init)
            {
                SbcDecoderInit();
                _init = true;

                _config.Frequency = _frame.Frequency;
                _config.Mode = _frame.Mode;
                _config.SubBandNum = _frame.SubbandMode;
                _config.BlockNum = _frame.BlockMode;
                _config.AllocationMethod = _frame.Allocation;
                _config.Bitpool = _frame.Bitpool;

                _frame.Codesize = SbcCommon.SbcGetCodesize(_config, _init, _frame);
                _frame.Length = (ushort)framelen;
            }
            else if (_frame.Bitpool != _config.Bitpool)
            {
                _frame.Length = (ushort)framelen;
                _config.Bitpool = _frame.Bitpool;
            }

            if (output == null)
                return framelen;

            int samples = SbcSynthesizeAudio();

            int pos = outputPos;
            int len = outputLen;
            if (output.Length - outputPos < outputLen)
                len = output.Length - outputPos;

            if (len < samples * _frame.Channels * 2)
                samples = len / (_frame.Channels * 2);

            for (int i = 0; i < samples; i++)
            {
                for (int ch = 0; ch < _frame.Channels; ch++)
                {
                    short s = _frame.PcmSample[ch, i];

                    if (_config.DataEndianess == DataEndianessEnum.SBC_BE)
                    {
                        output[pos++] = (byte)((s & 0xff00) >> 8);
                        output[pos++] = (byte)(s & 0x00ff);
                    }
                    else
                    {
                        output[pos++] = (byte)(s & 0x00ff);
                        output[pos++] = (byte)((s & 0xff00) >> 8);
                    }
                }
            }

            written = samples * _frame.Channels * 2;

            return framelen;
        }
        private int SbcUnpackFrame(byte[] data, int pos, int len)
        {
            int maxLen = len;
            if (maxLen > data.Length)
                maxLen = data.Length;

            if (maxLen - pos < 4)
                return -1;

            if (data[pos] != SbcCommon.SBC_SYNCWORD)
                return -2;

            _frame.Frequency = (FrequencyEnum)(data[pos + 1] >> 6 & 0x03);
            _frame.BlockMode = (BlocksEnum)(data[pos + 1] >> 4 & 0x03);

            switch (_frame.BlockMode)
            {
                case BlocksEnum.SBC_BLK_4:
                    _frame.Blocks = 4;
                    break;
                case BlocksEnum.SBC_BLK_8:
                    _frame.Blocks = 8;
                    break;
                case BlocksEnum.SBC_BLK_12:
                    _frame.Blocks = 12;
                    break;
                case BlocksEnum.SBC_BLK_16:
                    _frame.Blocks = 16;
                    break;
            }

            _frame.Mode = (ChannelModeEnum)(data[pos + 1] >> 2 & 0x03);

            switch (_frame.Mode)
            {
                case ChannelModeEnum.SBC_MODE_MONO:
                    _frame.Channels = 1;
                    break;
                case ChannelModeEnum.SBC_MODE_DUAL_CHANNEL:
                case ChannelModeEnum.SBC_MODE_STEREO:
                case ChannelModeEnum.SBC_MODE_JOINT_STEREO:
                    _frame.Channels = 2;
                    break;
            }

            _frame.Allocation = (AllocationMethodEnum)(data[pos + 1] >> 1 ^ 0x01);

            _frame.SubbandMode = (SubbandsEnum)(data[pos + 1] & 0x01);
            _frame.Subbands = (byte)(_frame.SubbandMode == SubbandsEnum.SBC_SB_8 ? 8 : 4);

            _frame.Bitpool = data[pos + 2];

            if ((_frame.Mode == ChannelModeEnum.SBC_MODE_MONO || _frame.Mode == ChannelModeEnum.SBC_MODE_DUAL_CHANNEL) && _frame.Bitpool > 16 * _frame.Subbands)
                return -4;

            if ((_frame.Mode == ChannelModeEnum.SBC_MODE_STEREO || _frame.Mode == ChannelModeEnum.SBC_MODE_JOINT_STEREO) && _frame.Bitpool > 32 * _frame.Subbands)
                return -4;

            return SbcUnpackFrameInternal(data, pos, maxLen);
        }
        private int SbcUnpackFrameInternal(byte[] data, int pos, int maxLen)
        {
            int len = maxLen - pos;
            byte[] crc_header = new byte[11];
            int temp;

            uint audio_sample;
            int ch, sb, blk, bit;

            int[,] bits = new int[2, 8];
            uint[,] levels = new uint[2, 8];

            uint consumed = 32;

            crc_header[0] = data[pos + 1];
            crc_header[1] = data[pos + 2];
            int crc_pos = 16;

            if (_frame.Mode == ChannelModeEnum.SBC_MODE_JOINT_STEREO)
            {
                if (len * 8 < consumed + _frame.Subbands)
                    return -1;

                _frame.Joint = 0x0;
                for (sb = 0; sb < _frame.Subbands - 1; sb++)
                    _frame.Joint |= (byte)((data[pos + 4] >> 7 - sb & 0x01) << sb);
                if (_frame.Subbands == 4)
                    crc_header[crc_pos / 8] = (byte)(data[pos + 4] & 0xf0);
                else
                    crc_header[crc_pos / 8] = data[pos + 4];

                consumed += _frame.Subbands;
                crc_pos += _frame.Subbands;
            }

            if (len * 8 < consumed + 4 * _frame.Subbands * _frame.Channels)
                return -1;

            for (ch = 0; ch < _frame.Channels; ch++)
            {
                for (sb = 0; sb < _frame.Subbands; sb++)
                {
                    _frame.ScaleFactor[ch, sb] = (byte)(data[pos + (int)(consumed >> 3)] >> 4 - (int)(consumed & 0x7) & 0x0F);
                    crc_header[crc_pos >> 3] |= (byte)(_frame.ScaleFactor[ch, sb] << 4 - (crc_pos & 0x7));

                    consumed += 4;
                    crc_pos += 4;
                }
            }

            if (data[pos + 3] != SbcCommon.SbcCrc8(crc_header, crc_pos))
                return -3;

            SbcCommon.SbcCalculateBits(_frame, bits);

            for (ch = 0; ch < _frame.Channels; ch++)
            {
                for (sb = 0; sb < _frame.Subbands; sb++)
                    levels[ch, sb] = (uint)((1 << bits[ch, sb]) - 1);
            }

            for (blk = 0; blk < _frame.Blocks; blk++)
            {
                for (ch = 0; ch < _frame.Channels; ch++)
                {
                    for (sb = 0; sb < _frame.Subbands; sb++)
                    {
                        uint shift;

                        if (levels[ch, sb] == 0)
                        {
                            _frame.SbSample[blk, ch, sb] = 0;
                            continue;
                        }

                        shift = _frame.ScaleFactor[ch, sb] + 1 + SBCDEC_FIXED_EXTRA_BITS;

                        audio_sample = 0;
                        for (bit = 0; bit < bits[ch, sb]; bit++)
                        {
                            if (consumed > len * 8)
                                return -1;

                            if ((data[pos + (int)(consumed >> 3)] >> 7 - (int)(consumed & 0x7) & 0x01) != 0)
                                audio_sample |= (uint)(1 << bits[ch, sb] - bit - 1);

                            consumed++;
                        }

                        _frame.SbSample[blk, ch, sb] = (int)((((ulong)audio_sample << 1 | 1) << (int)shift) / levels[ch, sb]) - (1 << (int)shift);
                    }
                }
            }

            if (_frame.Mode == ChannelModeEnum.SBC_MODE_JOINT_STEREO)
            {
                for (blk = 0; blk < _frame.Blocks; blk++)
                {
                    for (sb = 0; sb < _frame.Subbands; sb++)
                    {
                        if ((_frame.Joint & 0x01 << sb) != 0)
                        {
                            temp = _frame.SbSample[blk, 0, sb] + _frame.SbSample[blk, 1, sb];
                            _frame.SbSample[blk, 1, sb] = _frame.SbSample[blk, 0, sb] - _frame.SbSample[blk, 1, sb];
                            _frame.SbSample[blk, 0, sb] = temp;
                        }
                    }
                }
            }

            if ((consumed & 0x7) != 0)
                consumed += 8 - (consumed & 0x7);

            return (int)(consumed >> 3);
        }
    }
}
